/*
 * Copyright (c) Thorben Linneweber and others
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

using System;
using Jitter2.Dynamics;
using Jitter2.LinearMath;

using SoftFloat;

namespace Jitter2.SoftBodies
{
	
	public class SoftBodyTriangle : SoftBodyShape
	{
	    private readonly RigidBody v1;
	    private readonly RigidBody v2;
	    private readonly RigidBody v3;
	
	    public RigidBody Vertex1 => v1;
	    public RigidBody Vertex2 => v2;
	    public RigidBody Vertex3 => v3;
	
	    private sfloat halfThickness = (sfloat)0.05f;
	
	    public sfloat Thickness
	    {
	        get => halfThickness * (sfloat)2.0f;
	        set => halfThickness = value * (sfloat)0.5f;
	    }
	
	    public SoftBodyTriangle(SoftBody body, RigidBody v1, RigidBody v2, RigidBody v3)
	    {
	        this.v1 = v1;
	        this.v2 = v2;
	        this.v3 = v3;
	
	        SoftBody = body;
	        UpdateWorldBoundingBox(sfloat.Zero);
	    }
	
	    public override NVector3 Velocity => (sfloat)1.0f / (sfloat)3.0f * (v1.Data.Velocity + v2.Data.Velocity + v3.Data.Velocity);
	
	    public override RigidBody GetClosest(in NVector3 pos)
	    {
	        sfloat len1 = (pos - v1.Position).LengthSquared();
	        sfloat len2 = (pos - v2.Position).LengthSquared();
	        sfloat len3 = (pos - v3.Position).LengthSquared();
	
	        if (len1 < len2 && len1 < len3)
	        {
	            return v1;
	        }
	
	        if (len2 < len3 && len2 <= len1)
	        {
	            return v2;
	        }
	
	        return v3;
	    }
	
	    public override void UpdateWorldBoundingBox(sfloat dt /*= (sfloat)0.0f*/)
	    {
	        sfloat extraMargin = libm.Max(halfThickness, (sfloat)0.01f);
	
	        JBBox box = JBBox.SmallBox;
	
	        box.AddPoint(Vertex1.Position);
	        box.AddPoint(Vertex2.Position);
	        box.AddPoint(Vertex3.Position);
	
	        // prevent a degenerate bounding box
	        NVector3 extra = new NVector3(extraMargin);
	        box.Min -= extra;
	        box.Max += extra;
	
	        WorldBoundingBox = box;
	    }
	
	    public override void SupportMap(in NVector3 direction, out NVector3 result)
	    {
	        NVector3 a = v1.Position;
	        NVector3 b = v2.Position;
	        NVector3 c = v3.Position;
	
	        sfloat min = NVector3.Dot(a, direction);
	        sfloat dot = NVector3.Dot(b, direction);
	
	        result = a;
	
	        if (dot > min)
	        {
	            min = dot;
	            result = b;
	        }
	
	        dot = NVector3.Dot(c, direction);
	
	        if (dot > min)
	        {
	            result = c;
	        }
	
	        result += NVector3.Normalize(direction) * halfThickness;
	    
	        result += centerOffset;// 应用中心偏移
		}
	
	    public override void GetCenter(out NVector3 point)
	    {
	        point = ((sfloat)1.0f / (sfloat)3.0f) * (Vertex1.Position + Vertex2.Position + Vertex3.Position);
	    }
	}
}
